﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Research.Kinect;
using OpenCvSharp;
using Microsoft.Research.Kinect.Nui;
using System.Diagnostics;
namespace NIP.VisionSharp
{
    public class KinectSharp
    {
        public const int ARMCAM = 1;
        CvCapture ArmCap;
        public int RES_X { set; get; }
        public int RES_Y { set; get; }
        Runtime nui;
        int totalFrames = 0;
        int lastFrames = 0;
        DateTime lastTime = DateTime.MaxValue;

        int[] depthData ; 

        public KinectSharp()
        {
            RES_X = 320;
            RES_Y = 240;
            depthData = new int[RES_X * RES_Y];
            setupKinect(); 
        }

        ~KinectSharp()
        {
            if (nui != null)
            {
                //nui.Uninitialize();
            }
        }
        public void closeKinect()
        {
            nui.Uninitialize();
        }

        void setupArmCam()
        {
            ArmCap = Cv.CreateCameraCapture(ARMCAM);
        }

        int[] convertDepthFrame(byte[] depthFrame16)
        {
            for (int i16 = 0; i16 < depthFrame16.Length; i16 += 2)
            {
                depthData[(i16)/2] = depthFrame16[i16 + 1]*2^5 + (depthFrame16[i16] >> 3);
            }
            return depthData;

        }
        void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
        {
            PlanarImage Image = e.ImageFrame.Image;
            convertDepthFrame(Image.Bits);
            var img = new IplImage(new CvSize(RES_X, RES_Y), BitDepth.U16, 1);

            //for (int i = 0; i < RES_Y; i++)
            //{
            //    for (int j = 0; j < RES_X; j++)
            //    {
            //        Debug.Write(" " + depthData[i*RES_Y + j]);
            //    }
            //    Debug.WriteLine(" END LINE ");
            //}
            //Debug.WriteLine(" END IMAGE ");
        }

        public int setupKinect()
        {
            nui = new Runtime();

            try
            {
                nui.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor);
            }
            catch (InvalidOperationException)
            {
                Debug.WriteLine("Runtime initialization failed. Please make sure Kinect device is plugged in.");
                return 1;
            }


            try
            {
                nui.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
                nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);
            }
            catch (InvalidOperationException)
            {
                Debug.WriteLine("Failed to open stream. Please make sure to specify a supported image type and resolution.");
                return 1 ;
            }

            lastTime = DateTime.Now;

            nui.DepthFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_DepthFrameReady);
            return 0;

        }

        //public void gotDepthAlert();

        //public void gotRGBAlert();

        //public void RGBProc(IplImage image);

        //public ushort DepthProc(IplImage image);

        //public IplImage GetThresholdedImage(IplImage img);

        //public CvRect Calibration(IplImage thresh, IplImage rgb);

        //public void detectBlobs(IplImage frame, CvPoint top, CvPoint bot);

        //public void cameraCalib();

        //public void undistortFeed(IplImage feed);

        //public void getRGB(IplImage RGB);

        //public void getDepth(ushort[,] depthData, IplImage depth, IplImage depthgrey);

        //public void getPixelData(ushort depthData, IplImage temp);

        //public IplImage getArmCam();

    }
}
